package io.gitee.vexlife.snakeagentexample.astar;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.Random;

import io.gitee.vexlife.snake.GameController;
import io.gitee.vexlife.snakeagentexample.astar.AStarNode;
import io.gitee.vexlife.snake.basic.Direction;
import io.gitee.vexlife.snake.basic.Node;

public class AStar {

	private ArrayList<Node> trace;

	public AStar() {
		this.trace = new ArrayList<Node>();
	}

	public Direction AStarSearch(Node nStart, Node nDest, GameController state, boolean farthest, boolean preference) {
		ArrayList<AStarNode> closedSet = new ArrayList<AStarNode>();
		ArrayList<AStarNode> openSet = new ArrayList<AStarNode>();
		AStarNode start = AStarNode.fromNode(nStart);
		AStarNode dest = AStarNode.fromNode(nDest);
		start.setF(this.getDistanceBetween(start, dest));
		start.setG(0);
		openSet.add(start);
		while (openSet.size() != 0) {
			AStarNode current = this.getBestFNode(openSet, farthest, preference);
			openSet.remove(current);
			closedSet.add(current);
			for (int[] dir : new int[][] { { 0, 1 }, { 0, -1 }, { -1, 0 }, { 1, 0 } }) {
				AStarNode neighbour = fetchNodeFromSets(openSet, closedSet, current.getX() + dir[0],
						current.getY() + dir[1]);
				if (neighbour.equals(dest)) {
					neighbour.setParent(current);
					this.trace.clear();
					while (neighbour.getParent().getParent() != null) {
						this.trace.add(neighbour);
						neighbour = neighbour.getParent();
					}
					if (this.trace.size() == 0)
						this.trace.add(neighbour);
					this.trace.add(neighbour.getParent());
					if (neighbour.getX() - neighbour.getParent().getX() == 0)
						return neighbour.getY() - neighbour.getParent().getY() == 1 ? Direction.DOWN : Direction.UP;
					else
						return neighbour.getX() - neighbour.getParent().getX() == 1 ? Direction.RIGHT : Direction.LEFT;
				}
				if (!state.isEmpty(neighbour) || closedSet.contains(neighbour) || this.trace.contains(neighbour))
					continue;
				Double tentative_gScore = current.getG() + 1;
				if (!openSet.contains(neighbour)) {
					openSet.add(neighbour);
				} else if (tentative_gScore >= neighbour.getG()) {
					continue;
				}
				neighbour.setParent(current);
				neighbour.setG(tentative_gScore);
				neighbour.setF(tentative_gScore + this.getDistanceBetween(neighbour, dest));
			}
		}
		return null;
	}

	private AStarNode fetchNodeFromSets(ArrayList<AStarNode> openSet, ArrayList<AStarNode> closedSet, int x, int y) {
		AStarNode result = new AStarNode(x, y, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
		int index = openSet.indexOf(result);
		if (index != -1)
			result = openSet.get(index);
		index = closedSet.indexOf(result);
		if (index != -1)
			result = closedSet.get(index);
		return result;
	}

	/**
	 * @param start 起点的节点
	 * @param dest  终点的节点
	 * @return 两节点之间的几何距离
	 */
	private Double getDistanceBetween(Node start, Node dest) {
		return 0.0 + Math.abs(dest.getX() - start.getX()) + Math.abs(dest.getY() - start.getY());
	}

	public AStarNode getBestFNode(ArrayList<AStarNode> nodes, boolean farthest, boolean preference) {
		Double extremum = farthest ? Double.NEGATIVE_INFINITY : Double.POSITIVE_INFINITY;
		ArrayList<AStarNode> best = new ArrayList<AStarNode>();
		for (AStarNode k : nodes) {
			Double v = k.getF();
			if (farthest ? v > extremum : v < extremum) {
				extremum = v;
				best.clear();
				best.add(k);
			}
			if (v == extremum)
				best.add(k);
		}
		return best.get(preference ? 0 : (best.size() - 1));
	}

	public ArrayList<Node> getTrace() {
		return this.trace;
	}

	public void clearTrace() {
		this.trace.clear();
	}

	public Direction wander(Node nStart, Node nDest, GameController state) {
		AStarNode neighbour;
		ArrayList<AStarNode> openSet = new ArrayList<AStarNode>();
		for (int[] dir : new int[][] { { 0, 1 }, { 0, -1 }, { -1, 0 }, { 1, 0 } }) {
			neighbour = new AStarNode(nStart.getX() + dir[0], nStart.getY() + dir[1], 0, 0);
			if (!(state.isEmpty(neighbour) || state.getSnake().getTail().equals(neighbour)))
				continue;
			neighbour.setF(this.getSquaredDistanceBetween(neighbour, nDest));
			this.trace.clear();
			checkTail: {
				if (!state.getSnake().getTail().equals(neighbour)) {
					for (int i = 0; i < state.getSnake().getLength(); i++) {
						if (this.AStarSearch(neighbour, state.getSnake().getNodes().get(i), state, false, true) != null)
							break;
						this.trace.clear();
					}
					if (this.trace.size() - 1 >= state.getSnake().getLength())
						break checkTail;
					LinkedList<Node> bak = (LinkedList<Node>) state.getSnake().getNodes().clone();
					LinkedList<Node> now = state.getSnake().getNodes();
					for (int i = this.trace.size() - 2; i >= 0; i--) {
						now.removeFirst();
					}
					this.trace.clear();
					if (this.AStarSearch(neighbour, state.getSnake().getTail(), state, false, true) == null)
						neighbour.setF(neighbour.getF() - 1e10);
					state.getSnake().setNodes(bak);
				}
			}
			openSet.add(neighbour);
		}
		if(openSet.isEmpty()) {
			System.out.println("未找到可行的随意走路线！");
			return state.getSnake().getDirection();
		}
		neighbour = this.getBestFNode(openSet, true, true);
		if (neighbour.getX() - nStart.getX() == 0)
			return neighbour.getY() - nStart.getY() == 1 ? Direction.DOWN : Direction.UP;
		else
			return neighbour.getX() - nStart.getX() == 1 ? Direction.RIGHT : Direction.LEFT;
	}

	private double getSquaredDistanceBetween(Node i, Node j) {
		return Math.sqrt((i.getX()-j.getX())*(i.getX()-j.getX())+(i.getY()-j.getY())*(i.getY()-j.getY()));
	}
}